#include <sys/types.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <pthread.h>
#include <errno.h>
#include <error.h>
#include <sys/time.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h>
#include <wiringPi.h>
#include <wiringSerial.h>
#include <softPwm.h>

#define N 1024

#define run_car '1'
#define back_cak '2'
#define left_car '3'
#define right_car '4'
#define spin_left_car '5'
#define spin_right_car '6'
#define stop_car '0'

enum{
    enSTOP = 0,
    enRUN,
    enBACK,
    enLEFT,
    enRIGHT,
    enTLEFT,
    enTRIGHT,
}CarState;

int Left_motor_go = 28;
int Left_motor_back = 29;

int Right_motor_go = 24;
int Right_motor_back = 25;

int Left_motor_pwm = 27;
int Right_motor_pwm = 23;

int CarSpeedControl = 150;

char recvBuf[N] = {0};
char InputString[N] = {0};
int NewLineReceived = 0;


int g_Carstate = enSTOP;


void run()
{
    digitalWrite(Left_motor_go,HIGH);
    digitalWrite(Left_motor_back,LOW);
    softPwmWrite(Left_motor_pwm,CarSpeedControl);

    digitalWrite(Right_motor_go,HIGH);
    digitalWrite(Right_motor_back,LOW);
    softPwmWrite(Right_motor_pwm,CarSpeedControl);
}
void brake()
{
    digitalWrite(Left_motor_go,LOW);
    digitalWrite(Right_motor_back,LOW);
    digitalWrite(Right_motor_go,LOW);
    digitalWrite(Right_motor_back,LOW);
}

void left()
{
    digitalWrite(Left_motor_go,LOW);
    digitalWrite(Left_motor_back,LOW);
    softPwmWrite(Left_motor_pwm,0);

    digitalWrite(Right_motor_go,HIGH);
    digitalWrite(Right_motor_back,LOW);
    softPwmWrite(Right_motor_pwm,CarSpeedControl);
}

void right()
{
    digitalWrite(Left_motor_go,HIGH);
    digitalWrite(Left_motor_back,LOW);
    softPwmWrite(Left_motor_pwm,CarSpeedControl);

    digitalWrite(Right_motor_go,LOW);
    digitalWrite(Right_motor_back,LOW);
    softPwmWrite(Right_motor_pwm,0);
}

void spin_left()
{
    digitalWrite(Left_motor_go,LOW);
    digitalWrite(Left_motor_back,HIGH);
    softPwmWrite(Left_motor_pwm,CarSpeedControl);

    digitalWrite(Right_motor_go,HIGH);
    digitalWrite(Right_motor_back,LOW);
    softPwmWrite(Right_motor_pwm,CarSpeedControl);
}

void spin_right()
{
    digitalWrite(Left_motor_go,HIGH);
    digitalWrite(Left_motor_back,LOW);
    softPwmWrite(Left_motor_pwm,CarSpeedControl);

    digitalWrite(Right_motor_go,LOW);
    digitalWrite(Right_motor_back,HIGH);
    softPwmWrite(Right_motor_pwm,CarSpeedControl);
}

void back()
{
    digitalWrite(Left_motor_go,LOW);
    digitalWrite(Left_motor_back,HIGH);
    softPwmWrite(Left_motor_pwm,CarSpeedControl);

    digitalWrite(Right_motor_go,LOW);
    digitalWrite(Right_motor_back,HIGH);
    softPwmWrite(Right_motor_pwm,CarSpeedControl);
}
void setUavStat(int par1,int par2,int par3,int par4,int par5,int par6);
//飞控软件输入
void dosoftctlwork()
{
    switch(InputString[3])
    {
        case '1': 
            setUavStat(0,0,0,0,0,0);//设置飞行状态
            break;
        case '2':
            setUavStat(1,1,1,1,1,1);
            break;
    }
}
//飞控台输入
void dohardwarectlwork()
{
    switch(InputString[3])
    {
        case '1': 
            setUavStat(1,1,1,1,1,1);//设置飞行状态
            break;
        case '2':
            setUavStat(1,1,1,1,1,1);
            break;
    }
}
//输入端解析
void tcp_input_achie()
{
    if(InputString[0] == '$' && InputString[14] == '#')
    {
        if(InputString[3] == '1')
        {
            dosoftctlwork();
        }
        else if(InputString[3] == '2')
        {
            dohardawrectlwork();
        }
    }
}


//do recv
void* actHost(void* arg)
{
    int n=0;
    int sockfd = *(int *)arg;

    while(1)
    {
        memset(recvBuf,0,sizeof(recvBuf));
        n = recv(sockfd,recvBuf,sizeof(recvBuf),0);
        if(n<0){
            perror("fail to recv!\n");
            break;
        }
        else if(n==0){
            printf("connect has been closed\n");
            break;
        }
        else
        {
            printf("Recv %d bytes : %s\n",n,recvBuf);
        }
        //判断报文是否正确
        if(recvBuf[0] == '$'&& recvBuf[14] =='#')
        {
            NewLineReceived = 1;
            strcpy(InputString,recvBuf);
        }
        else
        {
            NewLineReceived = 0;
        }
        if(NewLineReceived == 1)
        {
            tcp_input_achie();
        }
    }
    close(sockfd);
    free(arg);
    pthread_exit(NULL);
}


int main(int argc,char* argv[])
{
    char buf[1024] = {0};

    int listenfd = 0;
    int connect_fd = 0;
    struct sockaddr_in hAddr;
    struct sockaddr_in cAddr;

    socklen_t solen = sizeof(hAddr);

    int n = 0;
    int *pconnect_fd = NULL;

    pthread_t tid;
    int ret = 0;

    //init hardware
    wiringPiSetup();

    pinMode(Left_motor_go, OUTPUT);
    pinMode(Left_motor_back, OUTPUT);
    pinMode(Right_motor_go, OUTPUT);
    pinMode(Right_motor_back, OUTPUT);
  
    softPwmCreate(Left_motor_pwm,0,255); 
    softPwmCreate(Right_motor_pwm,0,255);



    //创建监听套接字
    listenfd = socket(AF_INET,SOCK_STREAM,0);
    if(listenfd <0)
    {
        perror("fail to socket listen");
        exit(EXIT_FAILURE);
    }
    int opt = 1024;
    setsockopt(listenfd,SOL_SOCKET,SO_REUSEADDR,&opt,sizeof(int));
    bzero(&hAddr,solen);
    hAddr.sin_family = AF_INET;
    hAddr.sin_addr.s_addr = inet_addr("192.168.50.1");
    hAddr.sin_port = htons(atoi("8888"));
    //绑定监听的端口号和ip
    if(bind(listenfd,(struct sockaddr*)&hAddr,solen) < 0)
    {
        perror("fail to bind\n");
        exit(1);
    }
    //开始监听
    listen(listenfd,10);
    printf("listen....");

    while(1)
    {
        //这里用while循环的原因是，accept函数如果没有收到新的连接会堵塞
        pconnect_fd = (int *)malloc(sizeof(int));
        //建立新的连接，返回通信套接字
        *pconnect_fd = accept(listenfd,(struct sockaddr*)&cAddr,&solen);
        if(*pconnect_fd < 0)
        {
            perror("fail to accept");
            exit(1);
        }
        printf("===========================\n");
        printf("connected_fd :%d\n",*pconnect_fd);
        printf("client IP: %s\n",inet_ntoa(cAddr.sin_addr));
        printf("client Port: %d\n",ntohs(cAddr.sin_port));
        //创建新线程，新线程也就是actHost函数
        ret = pthread_create(&tid,NULL,actHost,(void*)pconnect_fd);
        if(ret!=0)
        {
            perror("fail to create thread\n");
            exit(1);
        }
        pthread_join(tid,NULL);


    }
    close(listenfd);
    return 0;
}
